﻿using System;
using System.Collections.Generic;
using LatoolNet;

namespace NumericalSolution {
  class Poisson2DFEM {
    private int m_numberOfElements;
    private int m_numberOfNodes;
    private int m_numberOfNodesInAnElement;
    private NumericalIntegration m_integration;

    private Element[] m_elements;
    private Dictionary<int, Node> m_nodeMap = new Dictionary<int, Node>();
    private Matrix m_A;
    private Matrix m_b;


    public int NumberOfElements {
      get { return m_numberOfElements; }
      set {
        m_numberOfElements = value;
        m_numberOfNodes = value + 1;
      }
    }

    public int NumberOfNodes {
      get { return m_numberOfNodes; }
    }

    public int NumberOfNodesInAnElement {
      get { return m_numberOfNodesInAnElement; }
      set { m_numberOfNodesInAnElement = value; }
    }

    public NumericalIntegration NumericalIntegration {
      get { return m_integration; }
      set { m_integration = value; }

    }
    public void CreateMesh() {

      m_elements = new Element[m_numberOfElements * m_numberOfElements];

      double x_begin = 0.0;
      double x_end = 1.0;
      double x_delta = (x_end - x_begin) / m_numberOfElements;

      double y_begin = 0.0;
      double y_end = 1.0;
      double y_delta = (y_end - y_begin) / m_numberOfElements;

      for (int i = 0; i < m_numberOfElements * m_numberOfElements; i++) {
        int col = i % m_numberOfElements;
        int row = i / m_numberOfElements;

        Element e = new Element(m_numberOfNodesInAnElement);
        m_elements[i] = e;
        e.Number = i;
        e.Node[0].X = col * x_delta;
        e.Node[0].Y = row * y_delta;
        e.Node[1].X = col * x_delta + x_delta;
        e.Node[1].Y = row * y_delta;
        e.Node[2].X = col * x_delta + x_delta;
        e.Node[2].Y = row * y_delta + y_delta;
        e.Node[3].X = col * x_delta;
        e.Node[3].Y = row * y_delta + y_delta;

        e.Node[0].Index = row * m_numberOfNodes + col;
        e.Node[1].Index = row * m_numberOfNodes + col + 1;
        e.Node[2].Index = row * m_numberOfNodes + col + m_numberOfNodes + 1;
        e.Node[3].Index = row * m_numberOfNodes + col + m_numberOfNodes;

        for (int n = 0; n < m_numberOfNodesInAnElement; n++) {
          if (m_nodeMap.ContainsKey(e.Node[n].Index) == false) {
            m_nodeMap.Add(e.Node[n].Index, e.Node[n]);
          }
        }

      }
    }

    public void AssembleMatrix() {

      m_A = new Matrix(m_numberOfNodes * m_numberOfNodes, 
                       m_numberOfNodes * m_numberOfNodes, 
                       m_numberOfNodes + 1, m_numberOfNodes + 1);
      m_b = new Matrix(m_numberOfNodes * m_numberOfNodes, 1);

      for (int e = 0; e < m_elements.Length; e++) {
        Matrix A_e = new Matrix(m_numberOfNodesInAnElement, 
                                m_numberOfNodesInAnElement);
        Matrix b_e = new Matrix(m_numberOfNodesInAnElement, 1);

        Element element = m_elements[e];

        AssembleLocalMatrix(A_e, b_e, element);
        AssembleGlobalMatrix(A_e, b_e, element);

        A_e.Dispose();
        b_e.Dispose();
      }

    }

    private void AssembleLocalMatrix(Matrix A_e, 
                                     Matrix b_e, 
                                     Element element) {

      for (int p = 0; p < m_integration.NumberOfIntegrationPoints; p++) {
        for (int q = 0; q < m_integration.NumberOfIntegrationPoints; q++) {

          Integrands(A_e, b_e, element,
                     m_integration.IntegrationPoint(p),
                     m_integration.Weight(p),
                     m_integration.IntegrationPoint(q),
                     m_integration.Weight(q));
        }
      }
    }

    private void Integrands(Matrix A_e, Matrix b_e, Element element,
                            double xi, double xi_weight,
                            double eta, double eta_weight) {

      double jacob11 = 0.0;
      double jacob12 = 0.0;
      double jacob21 = 0.0;
      double jacob22 = 0.0;

      for (int n = 0; n < m_numberOfNodesInAnElement; n++) {
        jacob11 += dPhi("dXi", n, xi, eta) * element.Node[n].X;
        jacob12 += dPhi("dXi", n, xi, eta) * element.Node[n].Y;
        jacob21 += dPhi("dEta", n, xi, eta) * element.Node[n].X;
        jacob22 += dPhi("dEta", n, xi, eta) * element.Node[n].Y;
      }

      double detJ = jacob11 * jacob22 - jacob12 * jacob21;

      double invJacob11 = jacob22 / detJ;
      double invJacob12 = -jacob12 / detJ;
      double invJacob21 = -jacob21 / detJ;
      double invJacob22 = jacob11 / detJ;

      for (int r = 0; r < m_numberOfNodesInAnElement; r++) {
        for (int s = 0; s < m_numberOfNodesInAnElement; s++) {

          double dPhiidX = invJacob11 * dPhi("dXi", r, xi, eta) +
                           invJacob12 * dPhi("dEta", r, xi, eta);
          double dPhiidY = invJacob21 * dPhi("dXi", r, xi, eta) +
                           invJacob22 * dPhi("dEta", r, xi, eta);
          double dPhijdX = invJacob11 * dPhi("dXi", s, xi, eta) +
                           invJacob12 * dPhi("dEta", s, xi, eta);
          double dPhijdY = invJacob21 * dPhi("dXi", s, xi, eta) +
                           invJacob22 * dPhi("dEta", s, xi, eta);

          A_e[r, s] += (dPhiidX * dPhijdX + dPhiidY * dPhijdY) *
                          detJ * xi_weight * eta_weight;

        }
        b_e[r, 0] += f(element.Node[r].X, element.Node[r].Y) 
                    * Phi(r, xi, eta) * detJ * xi_weight * eta_weight;

      }
    }

    private void AssembleGlobalMatrix(Matrix A_e, Matrix b_e, Element e) {

      int boundaryIndex;
      for (int i = 0; i < m_numberOfNodesInAnElement; i++) {
        if (Math.Abs(e.Node[i].X - 0.0) < 1e-10 ||
            Math.Abs(e.Node[i].X - 1.0) < 1e-10 ||
            Math.Abs(e.Node[i].Y - 0.0) < 1e-10 ||
            Math.Abs(e.Node[i].Y - 1.0) < 1e-10) {

          boundaryIndex = i;
          for (int j = 0; j < m_numberOfNodesInAnElement; j++) {
            if (j == boundaryIndex) {
              A_e[boundaryIndex, j] = 1;
            } else {
              A_e[boundaryIndex, j] = 0;
            }
          }
          b_e[boundaryIndex, 0] = 0;
        }
      }

      for (int r = 0; r < m_numberOfNodesInAnElement; r++) {
        for (int s = 0; s < m_numberOfNodesInAnElement; s++) {
          int rownum = e.Node[r].Index;
          int colnum = e.Node[s].Index;
          m_A[e.Node[r].Index, e.Node[s].Index] += A_e[r, s];
        }
        m_b[e.Node[r].Index, 0] += b_e[r, 0];
      }
    }

    public Node[] Solve() {

      //LUFactorization.Solve(m_A, m_b);
      LinearEquation.Solve(m_A, m_b);

      List<Node> nodelist = new List<Node>();
      for (int i = 0; i < m_numberOfNodes * m_numberOfNodes; i++) {
        Node node = m_nodeMap[i];
        node.Value = m_b[i, 0];
        nodelist.Add(node);
      }
      return nodelist.ToArray();
    }

    private double Phi(int n, double xi, double eta) {
      if (n == 0) {
        return 0.25 * (1 - xi) * (1 - eta);
      } else if (n == 1) {
        return 0.25 * (1 + xi) * (1 - eta);
      } else if (n == 2) {
        return 0.25 * (1 + xi) * (1 + eta);
      } else if (n == 3) {
        return 0.25 * (1 - xi) * (1 + eta);
      }
      throw new ArgumentOutOfRangeException();
    }

    private double dPhi(String der, int n, double xi, double eta) {
      if (der.Equals("dXi")) {
        if (n == 0) {
          return 0.25 * (-1 + eta);
        } else if (n == 1) {
          return 0.25 * (1 - eta);
        } else if (n == 2) {
          return 0.25 * (1 + eta);
        } else if (n == 3) {
          return 0.25 * (-1 - eta);
        }
      } else if (der.Equals("dEta")) {
        if (n == 0) {
          return 0.25 * (-1 + xi);
        } else if (n == 1) {
          return 0.25 * (-1 - xi);
        } else if (n == 2) {
          return 0.25 * (1 + xi);
        } else if (n == 3) {
          return 0.25 * (1 - xi);
        }
      }
      throw new ArgumentOutOfRangeException();
    }

    private double f(double x, double y) {
      return -2.0 * x * (x - 1) - 2.0 * y * (y - 1);
    }

  }
}
